from protobuf_sensor_msgs.sensor import ins_pb2
from protobuf_sensor_msgs.sensor import pointcloud_pb2
import folium
import sys
import argparse
import ecal.core.core as ecal_core
from ecal.core.subscriber import ProtoSubscriber
from flask import Flask, render_template, jsonify
from flask_socketio import SocketIO, emit
import time
import math

sys.path.append("..")

app = Flask(__name__)
socketio = SocketIO(app)

ins_lon = 119.600
ins_lat = 39.900
direction = 0  # 初始方向
light_lon = 119.631400
light_lat = 39.894583


def draw_triangle(lat, lon, direction):
    size = 0.001  # 增加三角形的长度
    width = 0.0002  # 三角形的宽度
    p1 = [lat + size * math.cos(direction * math.pi / 180), lon + size * math.sin(direction * math.pi / 180)]
    p2 = [lat + width * math.cos((direction + 135) * math.pi / 180), lon + width *
          math.sin((direction + 135) * math.pi / 180)]
    p3 = [lat + width * math.cos((direction - 135) * math.pi / 180), lon + width *
          math.sin((direction - 135) * math.pi / 180)]
    locations = [p1, p2, p3]
    return locations


def ins_callback(topic_name, msg, stamp):
    global ins_lon
    global ins_lat
    global direction
    ins_lat = msg.rtk_gps.llh.lat
    ins_lon = msg.rtk_gps.llh.lon
    direction = msg.imu.orientation.qx  # 模拟方向变化
    socketio.emit('ins_data', {'lat': ins_lat, 'lon': ins_lon, 'direction': direction})


def target_callback(topic_name, msg, stamp):
    global ins_lon
    global ins_lat
    global direction
    global light_lon
    global light_lat
    targets = []
    for point in msg.points:
        targets.append({'lat': point.lat, 'lon': point.lon})
    socketio.emit('target_data', {
        'ins_lat': ins_lat,
        'ins_lon': ins_lon,
        'direction': direction,
        'light_lat': light_lat,
        'light_lon': light_lon,
        'targets': targets
    })
    # print("target_callback")


def ecal_cloud_thread():
    topic = args.topic_ins
    ins_sub = ProtoSubscriber(topic, ins_pb2.INS)
    ins_sub.set_callback(ins_callback)
    print("Start ecal sub thread", topic)

    topic = args.topic_targets
    cloud_sub = ProtoSubscriber(topic, pointcloud_pb2.CloudLLH)
    cloud_sub.set_callback(target_callback)
    print("Start ecal sub thread", topic)

    while ecal_core.ok():
        time.sleep(1)


@app.route('/')
def index():
    return render_template('index.html')


if __name__ == '__main__':
    parser = argparse.ArgumentParser(prog='Pointcloud streamer',
                                     description="Pointcloud streaming with Flask and Three.js")
    parser.add_argument("--topic_targets", type=str, help="topic of targets", default="/map_targets")
    parser.add_argument("--topic_ins", type=str, help="topic of ins", default="/boat/ins")
    args = parser.parse_args()

    ecal_core.initialize(sys.argv, "llh_cloud_ins_viewer")
    socketio.start_background_task(ecal_cloud_thread)
    socketio.run(app, host='0.0.0.0', port=5000)
    ecal_core.finalize()
